#define DEBUG
#include "pins.h"
#include "car.h"
#include "detector.h"
//#include "button.h"
#include <SoftwareSerial.h>

#define BOUNDARY     20

Car *car;
Detector *detector;
//Button *left, *right;
bool leftBlocked = false, rightBlocked = false;
SoftwareSerial bluetooth (PIN_S_RX, PIN_S_TX);
double distance;

/*
void on_block_left (int value) {
    if (!value) {
        car->pause ();
        car->turnRight ();
        car->backward ();
        delay (200);
    }
}

void on_block_right (int value) {
    if (!value) {
        car->pause ();
        car->turnRight ();
        car->backward ();
        delay (200);
    }
    rightBlocked = (value != HIGH);
}
*/

void noop (int dummy) {}

void setup() {
    pinMode (PIN_LEFT_LED, OUTPUT);
    pinMode (PIN_RIGHT_LED, OUTPUT);
/*
    left  = new Button (PIN_L_SEN, 50, HIGH);
    right = new Button (PIN_R_SEN, 50, HIGH);
*/
    pinMode (PIN_L_SEN, INPUT);
    pinMode (PIN_R_SEN, INPUT);  
    Serial.begin (9600);
    bluetooth.begin (9600);

    car = new Car ();
    detector = new Detector ();
}

int readCommand () {
    int ch = Serial.read ();
    if (ch > 0) {
        switch (ch) {
            case 'f':
            case 'F':
                car->forward ();
                break;
            case 'b':
            case 'B':
                car->backward ();
                break;
            case 'p':
            case 'P':
                car->pause ();
                break;
            case 'r':
            case 'R':
                car->turnRight ();
                break;
            case 'l':
            case 'L':
                car->turnLeft ();
                break;
            case 'c':
            case 'C':
                car->resetDir ();
                break;
        }
    }
}

void loop() {
    readCommand ();
//    left ->check_state (noop);
//    right->check_state (noop);
    leftBlocked  = (digitalRead (PIN_L_SEN) == LOW);
    rightBlocked = (digitalRead (PIN_R_SEN) == LOW); 
#ifdef DEBUG
    Serial.print ("front-left blocked: ");
    Serial.print (leftBlocked);
    Serial.print (", front-right blocked: ");
    Serial.println (rightBlocked);
#endif

    if (leftBlocked && rightBlocked) {
#ifdef DEBUG
        Serial.println ("detected the block is in both of front-left and front-right, backward");
#endif
        car->resetDir ();
        car->backward ();
    } else if (leftBlocked) {
#ifdef DEBUG
        Serial.println ("Block is in front-left, we go right-back");
#endif
        car->turnLeft ();
        car->backward ();
    } else if (rightBlocked) {
#ifdef DEBUG
        Serial.println ("Block is in front-right, we go left-back");
#endif
        car->turnRight ();
        car->backward ();
    } else {
        distance = detector->getDistance ();
#ifdef DEBUG
        Serial.print ("front distance = ");
        Serial.println (distance);
#endif
        if (distance < 10) {
#ifdef DEBUG
        Serial.println ("the distance is less then 10, backward for 200ms.");
#endif
            car->resetDir ();
            car->backward ();
            delay (200);
        } else if (distance < 15) {
            car->resetDir ();
            car->pause ();
#ifdef DEBUG
        Serial.println ("the distance is less then 15, consider to backward.");
#endif
            double holder[3];
            detector->scan2 (holder);
            detector->reset ();
            int index = 0;
            double m  = holder [0];
            for (int i = 0; i < 3; i ++) {
#ifdef DEBUG
                Serial.print ("holder [");
                Serial.print (i);
                Serial.print ("] = ");
                Serial.println (holder [i]);
#endif
                if (m < holder [i]) {
                    index = i;
                    m = holder [i];
                }
            }
            if (m < 10) {
                switch (index) {
                    case 0 : // left
                        car->turnLeft ();
                        car->backward ();
                        break;
                    case 1 : // front
                        car->resetDir ();
                        car->backward ();
                        break;
                    case 2 : // right
                        car->turnRight ();
                        car->backward ();
                        break;
                }
            } else if (m < 15) {
                switch (index) {
                    case 0 : // left
                        car->turnRight ();
                        car->forward ();
                        break;
                    case 1 : // front
                        car->turnRight ();
                        car->forward ();
                        break;
                    case 2 : // right
                        car->turnLeft ();
                        car->forward ();
                        break;
                }
            }
            delay (200);
            
//            while (1);
        } else {
            car->resetDir ();
            car->forward ();
        }
    }
/*
    int dis_left = digitalRead (PIN_L_SEN);
    int dis_right = digitalRead (PIN_R_SEN);
*/
/*
#ifdef DEBUG
    Serial.print ("[");
    Serial.print (millis ());
    Serial.print ("] ");
    Serial.print ("dis_left = ");
    Serial.print (dis_left);
    Serial.print (", dis_right = ");
    Serial.println (dis_right);
#endif
*/
//    int dis_left = HIGH, dis_right = HIGH;
/*
    if (dis_left == LOW && dis_right == LOW) {
#ifdef DEBUG
        Serial.println ("backward");
#endif
        car->resetDir ();
        car->backward ();
    } else if (dis_left == LOW) {
#ifdef DEBUG
        Serial.println ("Block is in front-left, we go right-back");
#endif
        car->turnRight ();
        car->backward ();
    } else if (dis_right == LOW) {
#ifdef DEBUG
        Serial.println ("Block is in front-right, we go left-back");
#endif
        car->turnLeft ();
        car->backward ();
    } else {
#ifdef DEBUG
        Serial.print ("front distance = ");
        Serial.println (distance);
#endif
        if (distance < BOUNDARY) {
#ifdef DEBUG
            Serial.println ("That is too close, stop the car");
#endif
            car->pause ();
#ifdef DEBUG
            Serial.println ("scanning the distance...");
#endif
            int ret = detector->scan1 ();
            detector->reset ();
//                int ret = detector->scan ();
#ifdef DEBUG
            Serial.print ("ret = ");
            Serial.println (ret, DEC);
#endif
            switch (ret) {
                case -2 :   // right-back
#ifdef DEBUG
                    Serial.println ("right-back");
#endif
                    car->turnRight ();
                    car->backward ();
                    break;
                case -1 :   // left-back
#ifdef DEBUG
                    Serial.println ("left-back");
#endif
                    car->turnLeft ();
                    car->backward ();
                    break;
                case 0 :    // backward
#ifdef DEBUG
                    Serial.println ("backward");
#endif
                    car->resetDir ();
                    car->backward ();
                    break;
                case 1 :    // left-forward
#ifdef DEBUG
                    Serial.println ("left-forward");
#endif
                    car->turnLeft ();
                    car->forward ();
                    break;
                case 2 :    // right-forward
#ifdef DEBUG
                    Serial.println ("right-forward");
#endif
                    car->turnRight ();
                    car->forward ();
                    break;
                case 3 :    // forward
                    car->resetDir ();
                    car->forward ();
                    break;
            }

            delay (500);
//            }
        } else {
#ifdef DEBUG
            Serial.println ("forward");
#endif
            car->resetDir ();
            car->forward ();
        }
    }
*/
}
